/*************************************************************************
 * @File Name: main
 * @Author: Wangjiawei
 * @Version: 1.0
 * @Mail: 2531351704@qq.com
 * @Created Time: 2022/11/27
 * @Description 21年电赛F题智能送药小车
 ************************************************************************/
#include "stm32f10x.h"
#include "sys.h"
#define Debug 1 // 1为调试

int main()
{
	unsigned char num[6] = {0};
	unsigned char send_port[7] = {0X7F, 0X80, 0X01, 0X01, 0X00, 0X00, 0X81};
	float battery;
	int flag = 0;

	delay_init();
	NVIC_Config();
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
	TIM1_Init(3599, 99); //开启定时器1的5ms中断,f=200Hz=72000 000/3600/100;先失能

	yolov3_uart_init(115200);
	zigbee_uart_init(9600);
	Openmv_usart_init(115200);

	OLED_Init();
	OLED_Clear();

	Encoder_TIM2_Init();
	Encoder_TIM3_Init();

	Motor_Init();
	Servo_Init();
	PWM_Init_TIM1(0, 7199);

	Adc_Init();
	sensor_init();
	LED_Init();

	Servo_SetAngle(85); //舵机回正
	while (firstNum == 0)
		; //等待k210识别到数字
	while (rho == 0)
		; //等待openmv识别到红线
	while (read_sensor() == 0)
		; //等待有物体放入

	while (1)
	{
		OLED_Num2(0, 0, rho);
		OLED_Num2(0, 1, firstNum);
#if Debug

		/*******************************  近端  ******************************************/
		if (firstNum == 1) //识别到一号病房
		{
			switch (flag)
			{
			case 0:
			{
				Distant_GO(Go_near_bybios, 3500, Mode_Forward);
				open_loop(Go_near_openloop, 3500);
				flag = 1;
				delay_ms(1000);
				break;
			}
			case 1:
			{
				Car_turn(Turnleft_Pluse_Go, Mode_Left); //到达十字路口拐弯
				delay_ms(200);
				Distant_GO(Go_near_room, 3500, Mode_Forward); //拐弯后到达病房过程
				delay_ms(200);
				LED1(1);
				while (read_sensor() == 1)
					; //等待拿药
				delay_ms(200);
				LED1(0); //卸药完成
				flag = 2;
				break;
			}
			case 2:
			{
				open_loop(Go_near_room / 4, -3500); //倒回十字路口
				Distant_GO(Go_near_room * 3 / 4, 3500, Mode_Back);
				flag = 3;
				break;
			}
			case 3:
			{
				Car_turn(Turnleft_Pluse_Back, Mode_Left); //从十字路口回到出发点
				delay_ms(1000);
				Distant_GO(7900, 3500, Mode_Forward); //回到药房
				delay_ms(200);
				LED2(1);
				flag = 4;
				break;
			}
			default:
			{
				break;
			}
			}
		}
		else if (firstNum == 2) //识别到二号病房
		{
			switch (flag)
			{
			case 0:
			{
				Distant_GO(Go_near_bybios, 3500, Mode_Forward);
				open_loop(Go_near_openloop, 3500);
				flag = 1;
				break;
			}
			case 1:
			{
				Car_turn(Turnrignt_Pluse_Go, Mode_Right);
				delay_ms(1000);
				Distant_GO(Go_near_room, 3500, Mode_Forward);
				delay_ms(200);
				LED1(1);
				while (read_sensor() == 1)
					;
				delay_ms(200);
				LED1(0);
				flag = 2;
				break;
			}
			case 2:
			{
				open_loop(Go_near_room / 4, -3500);
				Distant_GO(Go_near_room * 3 / 4, 3500, Mode_Back);
				flag = 3;
				break;
			}
			case 3:
			{
				Car_turn(Turnright_Pluse_Back, Mode_Right);
				delay_ms(1000);
				Distant_GO(8500, 3500, Mode_Forward);
				delay_ms(200);
				LED2(1);
				flag = 4;
				break;
			}
			default:
			{
				break;
			}
			}
		}
		/*******************************  中端  ******************************************/
		else if (firstNum != 1 && firstNum != 2)
		{
			switch (flag)
			{
			case 0:
			{
				Distant_GO(Go_middle, 3500, Mode_Forward); //到达第二个十字路口
														   //						Servo_SetAngle(servo_Left);//向左偏头
														   //						load(0,0);
				delay_ms(1000);							   //等待舵机完成
				num[0] = numGet;
				flag = 1;
				break;
			}

			case 1:
			{

				if (firstNum == num[0]) //识别数字在左
				{
					open_loop(2500, 3500); //向前走到十字路口
					delay_ms(1000);
					Car_turn(Turnleft_Pluse_Go, Mode_Left);
					delay_ms(200);
					Distant_GO(Go_near_room, 3500, Mode_Forward);
					delay_ms(200);
					LED1(1);
					Servo_SetAngle(servo_straight);
					while (read_sensor() == 1)
						;
					delay_ms(200);
					LED1(0);
					open_loop(Go_near_room / 4, -3500); //倒回十字路口
					Distant_GO(Go_near_room * 3 / 4, 3500, Mode_Back);
					Car_turn(Turnleft_Pluse_Back, Mode_Left); //从十字路口回到出发点
					delay_ms(1000);
					Distant_GO(21000, 3500, Mode_Forward); //回到药房
					delay_ms(200);
					LED2(1);
					flag = 2;
					break;
				}
				else if (firstNum != num[0]) //识别数字不在左
				{
					Servo_SetAngle(servo_Right);
					load(0, 0);
					delay_ms(1500); //等待舵机完成
					num[1] = numGet;
					delay_ms(500);
					if (firstNum == num[1])
					{
						open_loop(2200, 3500); //向前走到十字路口
						Car_turn(Turnrignt_Pluse_Go, Mode_Right);
						delay_ms(1000);
						Distant_GO(Go_near_room, 3500, Mode_Forward);
						delay_ms(200);
						LED1(1);
						Servo_SetAngle(servo_straight);
						while (read_sensor() == 1)
							;
						delay_ms(200);
						LED1(0);
						open_loop(Go_near_room / 4, -3500); //倒回十字路口
						Distant_GO(Go_near_room * 3 / 4, 3500, Mode_Back);
						Car_turn(Turnright_Pluse_Back, Mode_Right); //从十字路口回到出发点
						delay_ms(1000);
						Distant_GO(21000, 3500, Mode_Forward); //回到药房
						delay_ms(200);
						LED2(1);
						flag = 2;
						break;
					}
					/*******************************  远端  ******************************************/
					else //不在中端
					{
						Distant_GO(11325, 3500, Mode_Forward);
						open_loop(3200, 3500); //到达第一个T字路口
						delay_ms(800);
						Car_turn(Turnleft_Pluse_Go, Mode_Left);
						delay_ms(200);
						Distant_GO(8700, 3000, Mode_Forward); //到达第二个T字路口
						delay_ms(800);
						num[2] = numGet;
						Servo_SetAngle(servo_Left - 10);
						load(0, 0);
						delay_ms(1500); //等待舵机完成
						num[3] = numGet;
						open_loop(2200, 3500); //向前走到十字路口
						delay_ms(1000);
						Car_turn(Turnleft_Pluse_Go, Mode_Left);
						delay_ms(500);
						Distant_GO(Go_near_room, 3500, Mode_Forward); //到达病房
						LED1(1);
						delay_ms(500);
						while (read_sensor() == 1)
							;
						int i;
						for (i = 0; i < 32; i++)
						{
							zigbee_send_array(USART2, send_port, 7); //发送数据给2车
						}
						delay_ms(200);
						LED1(0);
						//									 open_loop(Go_near_room / 4,-1500);//倒回十字路口

						open_loop(2300, -1500);
						Distant_GO(3000, 1500, Mode_Back);
						delay_ms(500);
						Car_turn(Turnleft_Pluse_Back - 100, Mode_Left);
						delay_ms(4000);
						Distant_GO(11700, 2000, Mode_Forward);
						delay_ms(500);
						Car_turn(Turnright_Pluse_Back - 500, Mode_Right); //从T字路口回到出发点
						delay_ms(800);
						Distant_GO(33000, 3500, Mode_Forward); //回到药房
						flag = 3;
						break;
					}
					if (firstNum != num[2] && firstNum != num[3]) //判断不是左边
					{
						Car_turn(Turnback, Mode_Left); //掉头
						delay_ms(800);
						Distant_GO(18000, 3500, Mode_Forward); //到右边T路口
						delay_ms(200);
						num[4] = numGet;
						OLED_Num2(0, 4, num[4]);
						Servo_SetAngle(servo_Right);
						load(0, 0);
						delay_ms(1500); //等待舵机完成
						num[5] = numGet;
						OLED_Num2(0, 5, num[5]);
						delay_ms(200);
						if (firstNum == num[4]) //在右上
						{
							open_loop(2400, 3500); //向前走到十字路口
							delay_ms(1000);
							Car_turn(Turnleft_Pluse_Go, Mode_Left);
							delay_ms(500);
							Distant_GO(Go_near_room, 3500, Mode_Forward); //到达病房
							LED1(1);
							delay_ms(500);
							while (read_sensor() == 1)
								;
							delay_ms(200);
							LED1(0);
							open_loop(Go_near_room / 5, -3500); //倒回十字路口
							Distant_GO(Go_near_room * 4 / 5, 3500, Mode_Back);
							delay_ms(500);
							Car_turn(Turnleft_Pluse_Back - 400, Mode_Left);
							Distant_GO(12000, 3500, Mode_Forward);
							delay_ms(500);
							Car_turn(Turnright_Pluse_Back - 500, Mode_Left); //从T字路口回到出发点
							delay_ms(800);
							Distant_GO(33000, 3500, Mode_Forward); //回到药房
							flag = 3;
							break;
						}
						if (firstNum == num[5]) //在右上
						{
							open_loop(2400, 3500); //向前走到十字路口
							delay_ms(1000);
							Car_turn(Turnrignt_Pluse_Go - 300, Mode_Right);
							delay_ms(500);
							Distant_GO(Go_near_room, 3500, Mode_Forward); //到达病房
							LED1(1);
							delay_ms(500);
							while (read_sensor() == 1)
								;
							delay_ms(200);
							LED1(0);
							open_loop(Go_near_room / 5, -3500); //倒回十字路口
							Distant_GO(Go_near_room * 4 / 5, 3500, Mode_Back);
							delay_ms(500);
							Car_turn(Turnright_Pluse_Back - 700, Mode_Right);
							Distant_GO(11500, 3500, Mode_Forward);
							delay_ms(500);
							Car_turn(Turnright_Pluse_Back - 500, Mode_Left); //从T字路口回到出发点
							delay_ms(800);
							Distant_GO(33000, 3500, Mode_Forward); //回到药房
							flag = 3;
							break;
						}
						flag = 3;
						break;
					}
					flag = 3;
					break;
				}
				break;
			}
			}
		}
	}
#endif
}
